# 二维码例程
#
# 这个例子展示了OpenMV Cam使用镜头校正来检测QR码的功能（请参阅qrcodes_with_lens_corr.py脚本以获得更高的性能）。
import sensor, image, time,math
import pyb
from pyb import LED,Timer,UART

#串口初始化
uart=UART(3,115200)
#通信部分代码
class target_check(object):
    para1 = 0   #uin8   第1个物料
    para2 = 0   #uin8   第2个物料
    para3 = 0   #uin8   第3个物料
    para4 = 0   #uin8   第4个物料
    para5 = 0   #uin8   第5个物料
    para6 = 0   #uin8   第6个物料
    para7 = 0   #uin8   标志位
    para8 = 0   #uin16  x误差
    para9 = 0   #uin16  y误差
    para10 = 0  #标志位


class rgb(object):
    def __init__(self):
        self.red=LED(1)
        self.green=LED(2)
        self.blue=LED(3)



class uart_buf_prase(object):
    uart_buf = []
    _data_len = 0
    _data_cnt = 0
    state = 0

class mode_ctrl(object):
    work_mode = 0 #工作模式.默认是点检测，可以通过串口设置成其他模式
    check_show = 1   #开显示，在线调试时可以打开，离线使用请关闭，可提高计算速度

ctr=mode_ctrl()
rgb=rgb()
R=uart_buf_prase()
pack=target_check()

HEADER=[0x11,0x13]
MODE=[0x01,0x02,0x03]
#__________________________________________________________________
def package_blobs_data():
    #数据打包封装
    data=bytearray([HEADER[0],HEADER[1],MODE[0],0x00,
                   pack.para1,#物料1
                   pack.para2,#物料2
                   pack.para3,#物料3
                   pack.para4,#物料4
                   pack.para5,#物料5
                   pack.para6,#物料6
                   pack.para7,#任务状态
                   pack.para8>>8,pack.para8&0xff,#x轴偏差
                   pack.para9>>8,pack.para9&0xff,#y轴偏差
                   pack.para10,#误差有效标志位
                   0x00])
    #数据包的长度
    data_len=len(data)
    data[3]=data_len-5#有效数据的长度
    #和校验
    sum=0
    for i in range(0,data_len-1):
        sum=sum+data[i]
    data[data_len-1]=sum%256
    #返回打包好的数据
    return data
#__________________________________________________________________

#串口数据解析
def Receive_Anl(data_buf,num):
    #和校验
    sum = 0
    i = 0
    while i<(num-1):
        sum = sum + data_buf[i]
        i = i + 1
    sum = sum%256 #求余
    if sum != data_buf[num-1]:
        return
    #和校验通过
    if data_buf[2]==0xA0:
        #设置模块工作模式
        ctr.work_mode = data_buf[4]
        #print(ctr.work_mode)
        #print("Set work mode success!")

#__________________________________________________________________
def uart_data_prase(buf):
    if R.state==0 and buf==0xFF:#帧头1
        R.state=1
        R.uart_buf.append(buf)
    elif R.state==1 and buf==0xFE:#帧头2
        R.state=2
        R.uart_buf.append(buf)
    elif R.state==2 and buf<0xFF:#功能字
        R.state=3
        R.uart_buf.append(buf)
    elif R.state==3 and buf<50:#数据长度小于50
        R.state=4
        R._data_len=buf  #有效数据长度
        R._data_cnt=buf+5#总数据长度
        R.uart_buf.append(buf)
    elif R.state==4 and R._data_len>0:#存储对应长度数据
        R._data_len=R._data_len-1
        R.uart_buf.append(buf)
        if R._data_len==0:
            R.state=5
    elif R.state==5:
        R.uart_buf.append(buf)
        R.state=0
        Receive_Anl(R.uart_buf,R.uart_buf[3]+5)
#        print(R.uart_buf)
        R.uart_buf=[]#清空缓冲区，准备下次接收数据
    else:
        R.state=0
        R.uart_buf=[]#清空缓冲区，准备下次接收数据

#__________________________________________________________________


def uart_data_read():
    buf_len=uart.any()
    for i in range(0,buf_len):
        uart_data_prase(uart.readchar())
#__________________________________________________________________

# -------------------------------------------------------------------
# 函数名:val_save_check(val)
# 传入参数:结果,要检查的值
# 功能:保存最近几次的结果(类似队列)，并检查出现的次数
# 返回值:出现次数大于设定值：数值，小于设定值：0
# ---------------------------------------------------------------------
last_val = [0,0,0,0,0,0,0,0,0,0]#历史数值保存列表
def val_save_check(reslut,val):
    global last_val
    last_val.pop(0)#弹出最后数据
    last_val.append(reslut)#更新最新的数据
    if last_val.count(val)>6:#查询数据的出现次数
        return val
    else:
        return 0


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_transpose(True)
#sensor.get_vflip()
sensor.set_hmirror(True)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)  # 必须关闭此功能，以防止图像冲洗…
clock = time.clock()


#抓取物块顺序
block_1_1 = 0
block_1_2 = 0
block_1_3 = 0
block_2_1 = 0
block_2_2 = 0
block_2_3 = 0
#运行时间
run_time = 0

#识别抓取物块的顺序
#1(红) 2(绿) 3(蓝)
def block_sort(a):
    global block_1_1,block_1_2,block_1_3,block_2_1,block_2_2,block_2_3
    block_1_1 = int(a[0])
    block_1_2 = int(a[1])
    block_1_3 = int(a[2])
    block_2_1 = int(a[4])
    block_2_2 = int(a[5])
    block_2_3 = int(a[6])

    pack.para1 = block_1_1
    pack.para2 = block_1_2
    pack.para3 = block_1_3
    pack.para4 = block_2_1
    pack.para5 = block_2_2
    pack.para6 = block_2_3


#二维码识别
def code_look():
    global task
    img = sensor.snapshot()
    #img.lens_corr(1.8) # 1.8的强度参数对于2.8mm镜头来说是不错的。
    for code in img.find_qrcodes():
        img.draw_rectangle(code.rect(), color = (255, 0, 0))
        block_sort(code.payload())
        #print(code.payload())
        #task = 3
        print(block_1_1,block_1_2,block_1_3,block_2_1,block_2_2,block_2_3)

#圆形识别
def circle_look():
    img = sensor.snapshot()
    c_x_err = 0
    c_y_err = 0
    cs = img.find_circles(threshold = 7000, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 30, r_max = 100, r_step = 2)
    if cs:
            for c in cs:
                img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))
                c_x_err = 120 - c.x()
                c_y_err = 160 - c.y()
                #print(c_x_err,c_y_err)
                if (math.sqrt(c_x_err*c_x_err + c_y_err*c_y_err)<20):
                    return 1,c_x_err,c_y_err
                else :
                    return 1,c_x_err,c_y_err
    else:
        return 0,c_x_err,c_y_err#没识别到


#寻找最大色块
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


#色块信息数据
c_x = 0
c_y = 0
c_x_change = 0
c_y_change = 0
c_x_last = 0
c_y_last = 0
c_x_err = 0
c_y_err = 0
thresholds_yallow = [(0, 100, -41, 32, 82, 58)]
thresholds_rad = [(0, 100, 9, 110, 119, 21)]
#3个货物阈值,顺序rgb分别为123
thresholds = [(0, 100, 97, 19, 1, 127),(0, 100, -70, -13, -64, 121),(0, 100, 119, -3, -128, -8)]
#寻找block
def find_block(thr):
    global c_x,c_y,c_x_change,c_y_change,c_x_last,c_y_last
    img = sensor.snapshot()
    blos = img.find_blobs(thr,merge = True)
    if blos:
        max_blos = find_max(blos)
        #计算中心坐标
        c_x = max_blos.x() + int(max_blos.w()/2)
        c_y = max_blos.y() + int(max_blos.h()/2)
        #计算变化率
        c_x_change = c_x - c_x_last
        c_y_change = c_y - c_y_last
        if abs(c_x_change)>=3 or abs(c_y_change)>=3:
            print("run")
            c_x_err = 0
            c_y_err = 0
        else:
            c_x_err = 160 - c_x
            c_y_err = 120 - c_y
            print("stop")
            print(c_x_err,c_y_err)
        #画出识别色块
        img.draw_rectangle(max_blos.rect())
        img.draw_cross(c_x, c_y)
        #print(c_x_change,c_y_change)
        #记录历史值
        c_x_last = c_x
        c_y_last = c_y

#转盘位置矫正
#检测任意暂停货物来纠正位置
def circle_position_correction():
    global c_x,c_y,c_x_change,c_y_change,c_x_last,c_y_last,thresholds
    c_x_err,c_y_err = 0,0
    flag = 0
    img = sensor.snapshot()
    for r in thresholds:
        blos = img.find_blobs([r],pixels_threshold  = 10000)
        if blos:
            max_blos = find_max(blos)
            #计算中心坐标
            c_x = max_blos.x() + int(max_blos.w()/2)
            c_y = max_blos.y() + int(max_blos.h()/2)
            #计算变化率
            c_x_change = c_x - c_x_last
            c_y_change = c_y - c_y_last
            if abs(c_x_change)>=3 or abs(c_y_change)>=3:
                print("run")
                c_x_err = 0
                c_x_err = 0
                flag = 0#误差无效
            else:
                c_x_err = 120 - c_x
                c_y_err = 160 - c_y
                flag = 1#误差有效
                print("stop")
            #画出识别色块
            img.draw_rectangle(max_blos.rect())
            img.draw_cross(c_x, c_y)
            #print(c_x_change,c_y_change)
            #记录历史值
            c_x_last = c_x
            c_y_last = c_y
            #找到其中一个货物,退出循环
            break
    #返回误差信息
    #print(flag,c_x_err,c_y_err)
    return flag,c_x_err,c_y_err

#检测货物暂停并抓取货物
#输入thr:要抓取的货物(红，绿，蓝分别为1,2,3)
#输出:是否抓取(1为抓取)
def get_block(block):
    global c_x,c_y,c_x_change,c_y_change,c_x_last,c_y_last,thresholds,waitting_flag
    #抓取标志位
    get_flag = 0
    first_block = 0#记录看到的第一个物块
    img = sensor.snapshot()
    blos = img.find_blobs([thresholds[block]],pixels_threshold  = 12000)
    if blos:
        max_blos = find_max(blos)
        #计算中心坐标
        c_x = max_blos.x() + int(max_blos.w()/2)
        c_y = max_blos.y() + int(max_blos.h()/2)
        #计算变化率
        c_x_change = c_x - c_x_last
        c_y_change = c_y - c_y_last
        if abs(c_x_change)>=3 or abs(c_y_change)>=3:
            print("run")
            #抓取标志位置0
            get_flag = 0
        else:
            print("stop")
            #抓取标志位置1
            get_flag = 1
        #画出识别色块
        img.draw_rectangle(max_blos.rect())
        img.draw_cross(c_x, c_y)
        #print(c_x_change,c_y_change)
        #记录历史值
        c_x_last = c_x
        c_y_last = c_y
    return get_flag
#---------------------------------------------------------------对叠
put_thresholds_end = [(0, 100, -70, -13, -64, 121)]
def get_block_end():
    global c_x,c_y,c_x_change,c_y_change,c_x_last,c_y_last,thresholds,waitting_flag,put_thresholds_end
    #抓取标志位
    get_flag = 0
    flag = 0
    first_block = 0#记录看到的第一个物块
    c_x_err = 0
    c_y_err = 0
    img = sensor.snapshot()
    blos = img.find_blobs(put_thresholds_end,pixels_threshold  = 6000)
    if blos:
            max_blos = find_max(blos)
            #计算中心坐标
            c_x = max_blos.x() + int(max_blos.w()/2)
            c_y = max_blos.y() + int(max_blos.h()/2)
            #计算变化率

            c_x_err = 120 - c_x
            c_y_err = 160 - c_y
            flag = 1#误差有效
            #画出识别色块
            img.draw_rectangle(max_blos.rect())
            img.draw_cross(c_x, c_y)
            #print(c_x_change,c_y_change)
            #记录历史值
            #找到其中一个货物,退出循环

    #返回误差信息
    #print(flag,c_x_err,c_y_err)
    return flag,c_x_err,c_y_err



#放置位置矫正(先色块矫正再圆环矫正)
#放置区中心圆环阈值
#矫正结束标志位
block_flag = 1
circle_flag = 0
put_thresholds = [(11, 43, -7, -29, -11, 49)]
def put_position_correction():
    global put_thresholds,block_flag,circle_flag
    c_x_err,c_y_err = 0,0
    img = sensor.snapshot()
    if block_flag == 0:#检测色块
        blos = img.find_blobs(put_thresholds,area_threshold  = 800,merge = True,margin = 50)
        if blos:
            max_blos = find_max(blos)
            #计算中心坐标
            c_x = max_blos.x() + int(max_blos.w()/2)
            c_y = max_blos.y() + int(max_blos.h()/2)
            c_x_err = 160 - c_x
            c_y_err = 120 - c_y
            #画出识别色块
            img.draw_rectangle(max_blos.rect())
            img.draw_cross(c_x, c_y)
            if(math.sqrt(c_x_err*c_x_err+c_y_err*c_y_err)<40):
                block_flag = 1#圆环矫正完成
    else:#检测圆环
        circle_flag,c_x_err,c_y_err = circle_look()
    #print(circle_flag,c_x_err,c_y_err)
    return circle_flag,c_x_err,c_y_err
#定时器
def time_callback(info):
    global run_time,task
    if task == 0:#黑
        rgb.green.off()
        rgb.red.off()
        rgb.blue.off()
    elif task == 1:#红灯亮
        rgb.green.off()
        rgb.red.toggle()
        rgb.blue.off()
    elif task == 9:#蓝灯亮
        rgb.blue.toggle()
        rgb.red.off()
        rgb.green.off()
    elif task == 2:#绿灯亮
        rgb.blue.on()
        rgb.red.on()
        rgb.green.on()
    else:
        rgb.blue.on()
        rgb.red.on()
        rgb.green.on()
    run_time = run_time + 1
timer=Timer(2,freq=10)#使用定时器2创建定时器对象-以4hz触发
timer.callback(time_callback)#将回调函数设置为time_callback函数


#任务状态机
task = 0
err_flag = 0#误差有效标志位
while(True):
    clock.tick()
    #任务状态机

    if task == 1:#二维码识别
        code_look()
    elif task == 2:#圆盘位置矫正
        err_flag,c_x_err,c_y_err = circle_position_correction()
    elif task == 3:#抓第1个物块
        pack.para7 = get_block(pack.para1 - 1)
    elif task == 4:#抓第2个物块
        pack.para7 = get_block(pack.para2 - 1)
    elif task == 5:#抓第3个物块
        pack.para7 = get_block(pack.para3 - 1)
    elif task == 6:#抓第4个物块
        pack.para7 = get_block(pack.para4 - 1)
    elif task == 7:#抓第5个物块
        pack.para7 = get_block(pack.para5 - 1)
    elif task == 8:#抓第6个物块
        pack.para7 = get_block(pack.para6 - 1)
    elif task == 9:#圆环位置矫正
        err_flag,c_x_err,c_y_err = put_position_correction()
    elif task == 10:
        err_flag,c_x_err,c_y_err = get_block_end()
    elif task == 0:#待机
        img = sensor.snapshot()
        err_flag,c_x_err,c_y_err,pack.para7 = 0,0,0,0

    #数据传递，发送数据
    pack.para8 = c_x_err
    pack.para9 = c_y_err
    pack.para10 = err_flag
    uart.write(package_blobs_data())
    uart_data_read()
    task = ctr.work_mode
    print(task)
        #find_block(thresholds_rad)
    print(pack.para7,c_x_err,c_y_err)
    #print(clock.fps())
